Gauss-newton-on-manifold for Pose Estimation

We consider the task of estimating the relative pose (position and orientation) between a 3D object and its projection on a 2D image plane from a set of point correspondences. Our approach is to formulate the task as an unconstrained optimization problem on the intersection of the special orthogonal group and a cone, and exploit as much as possible the geometry of the underlying parameter space. The optimization does not require Riemannian geometry. It involves successive parameterization of the constraint manifold and is based on Newton-type iterations in local parameter space. A direct proof of local quadratical convergence to the optimum is provided. A key feature of the proposed approach, not used in earlier studies, is an analytic geodesic search, alternating between gradient, Gauss, Newton and random directions, which ensures the escape from local minima and convergence to a global minimum without the need to reinitialize the algorithm. Indeed, for a prescribed number of iterations, the proposed algorithm achieves significantly lower pose estimation errors than earlier methods and it converges to a global minimum in typically 5–10 iterations.